# -*- coding: utf-8 -*-
#
# Copyright (C) 2024 reinovo, Inc. All Rights Reserved 
#
# @Time    : 2024/12/04 下午4:54
# @Author  : hmm
# @Email   : liuyuhang0531@foxmail.com
# @File    : rei_roslaunch.py

import rospy
import roslaunch
import os
from multiprocessing import Process,Queue
import time

class ReiRoslaunch():

    stateDict = ['CLOSE','PENDING','ACTIVE','SUCCEEDED','ABORTED']

    def __init__(self,launchList,is_core=False):
        self.uuid = roslaunch.rlutil.get_or_generate_uuid(None, False)
        roslaunch.configure_logging(self.uuid)
        self._runningNode = []
        self._state = Queue()
        self._close = Queue()
        self._closeNode = Queue()
        self._state.put({'time':time.time(),'state':self.stateDict[0],'message':'初始化rei_roslaunch'})
        self.launchList = self._create_launchList(launchList,is_core=is_core)
        auto_terminate = True
        self._log = []
        try:
            if is_core == True:
                os.system('killall -9 rosnode')
                os.system('killall -9 rosmaster')
                auto_terminate = False
            self.proc = Process(target=self._rosRunning,args=(self.launchList,auto_terminate,))
            self._state.put({'time':time.time(),'state':self.stateDict[1],'message':'rei_roslaunch注册成功'})
        except:
            self._state.put({'time':time.time(),'state':self.stateDict[0],'message':'rei_roslaunch初始化失败'})
        
    def shutdown(self):
        self._closeNode.put('close')
        for p in self._runningNode:
            p.join()
        self._close.put('close')
        self.proc.join()

    def start(self):
        if self.getState() != self.stateDict[0]:
            self.proc.start()
        else:
            self._state.put({'time':time.time(),'state':self.stateDict[0],'message':'rei_roslaunch未初始化'})

    def start_node(self,pkg,node,name=None):
        proc_node = Process(target=self._rosNodeRunning,args=(pkg,node,name,))
        self._runningNode.append(proc_node)
        proc_node.start()
    
    def _rosNodeRunning(self,pkg,node,name):
        launch = roslaunch.scriptapi.ROSLaunch()
        launch.parent = roslaunch.parent.ROSLaunchParent(self.uuid, [])
        launch.start()
        # Start another node
        node = roslaunch.core.Node(pkg, node, name)
        process = launch.launch(node)
        while not rospy.is_shutdown():
            if self._closeNode.get() == 'close':
                break
        process.stop()

    def _rosRunning(self,launchList,auto_terminate):
        self._state.put({'time':time.time(),'state':self.stateDict[1],'message':'rei_roslaunch启动中'})
        launchList.start(auto_terminate)
        self._state.put({'time':time.time(),'state':self.stateDict[2],'message':'rei_roslaunch已启动'})
        while not rospy.is_shutdown():
            if self._close.get() == 'close':
                break
        launchList.shutdown()
        self._state.put({'time':time.time(),'state':self.stateDict[0],'message':'rei_roslaunch已关闭'})

    def getState(self):
        while self._state.qsize() != 0:
            self._log.append(self._state.get())
        return self._log[-1]['state']

    def _add_launchFile(self,object_list,pkg,file,args=[]):
        """
        查找launch路径
        :param object_list: 启动列表，将在函数内修改
        :param pkg: 功能包名称
        :param file: launch文件名
        :param arg: launch的可选参数配置列表,["param1 := num1", "param2:=num2"] 
        :return: 添加是否成功
        """
        ros_path = os.environ['ROS_PACKAGE_PATH']
        cli_args = []
        for dir in ros_path.split(':'):
            path = os.path.join(dir,pkg)
            if os.path.exists(path):
                path = os.path.join(os.path.join(path,'launch'),file)
                if os.path.exists(path):
                    cli_args.append(path)
                    if len(args) == 0:
                        object_list.append(path)
                    else:
                        for arg in args:
                            cli_args.append(arg)
                        object_list.append((roslaunch.rlutil.resolve_launch_arguments(cli_args)[0],cli_args[1:]))
                    return True
                else:
                    self._state.put({'time':time.time(),'state':self.stateDict[1],'message':'未找到launch文件file:'+file})
            else:
                self._state.put({'time':time.time(),'state':self.stateDict[1],'message':'未找到pkg:'+pkg})
        return False
    
    def _create_launchList(self,launch,is_core=False):
        launchList = []
        for code in launch:
            # self._add_launchFile(launchList,code.split(' ')[1],code.split(' ')[2],code.split(' ')[3:])
            self._add_launchFile(launchList,code['pkg'],code['file'],code['args'])
        rosbridge_launch = roslaunch.parent.ROSLaunchParent(self.uuid,launchList,is_core=is_core,show_summary=True)
        return rosbridge_launch  

class robot_launch():
    def __init__(self,rei_robot=None):
        self.runList = []
        self.slam_is_active = False
        self.cfg = self._read_json_file()
        if rei_robot == None:
            try:
                self.rei_robot = os.environ['REI_ROBOT']
            except:
                print('未找到设备信息，请自定义')
        else:
            self.rei_robot = rei_robot

    def _read_json_file(self):
        import json
        try:
            with open('../src/config/reirobot.json', 'r') as file:
                data = json.load(file)
                return data
        except FileNotFoundError:
            print("config/reirobot.json:文件未找到")
        except json.JSONDecodeError:
            print("config/reirobot.json:文件JSON数据解析错误")
        return data
    
    def roslaunch(self,keys):
        self.stop()
        self.runList = []
        launchList = []
        self.running = keys
        for key in keys:
            launch = self.cfg['robotConfig'][self.rei_robot][key]
            launchList.append(launch)
        ros = ReiRoslaunch(launchList)
        self.runList.append(ros)

    def save_map(self,map_name):
        if 'slam' in self.running and self.getState()=="ACTIVE":
            launch = self.cfg['robotConfig'][self.rei_robot]['map_save']
            launch['args'].append('map_file:='+map_name)
            ros = ReiRoslaunch([launch])
            ros.start()
            self.runList.append(ros)

    def open_nav(self,map_name,is_core=False):
        self.stop()
        self.runList = []
        launch = self.cfg['robotConfig'][self.rei_robot]['nav']
        launch['args'].append('map_file:='+map_name)
        ros = ReiRoslaunch([launch],is_core=is_core)
        self.runList.append(ros)

    def start(self):
        for ros in self.runList:
            if ros.getState() == 'PENDING':
                ros.start()

    def stop(self):
        for ros in self.runList:
            if ros.getState() == 'ACTIVE':
                ros.shutdown()

    def getState(self):
        for ros in self.runList:
            return ros.getState()